#include <iostream>
#include <string>
#include <sstream>
#include <fstream>
#include <iomanip>

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <serial/serial.h>

#include <nav_msgs/Odometry.h>  
#include <tf/transform_broadcaster.h>

#include <time.h>

int main(int argc, char** argv)
{
	ros::init(argc, argv, "odom2txt");
	ros::NodeHandle nh;
	ros::NodeHandle private_nh("~");

    time_t tt = time(NULL);
    struct tm* t = localtime(&tt);

    std::stringstream filename;
    filename << "/home/nano/workspace/ai-robot/catkin_ws/" << "odom_" << t->tm_year + 1900 << "_"
                                                                    << t->tm_mon + 1 << "_" 
                                                                    << t->tm_mday << "_" 
                                                                    << t->tm_hour << "_" 
                                                                    << t->tm_min << "_" 
                                                                    << t->tm_sec << ".txt";
    std::ofstream output_file_s;
    output_file_s.open(filename.str().c_str());
    output_file_s.precision(15);

    auto startTime = ros::Time::now();

    ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry>("/odom", 10, [&](const nav_msgs::Odometry::ConstPtr& odom){
        double vx = odom->twist.twist.linear.x;
        double vy = odom->twist.twist.linear.y;
        double vth = odom->twist.twist.angular.z;
        auto quat = odom->pose.pose.orientation;
        auto th = tf::getYaw(quat);
        
        auto timestamp = odom->header.stamp;
        output_file_s   << (timestamp - startTime).toNSec() / 1e9 << " "
                        << vx << " " 
                        << vy << " "
                        << vth << " " 
                        << th << " " << std::endl;
                        
        ROS_INFO("%f, %f, %f", vx, vy, vth);

    });

    ros::Rate loop_rate(100);
    while(ros::ok()){
        ros::spinOnce();
        loop_rate.sleep();
    }
    output_file_s.close();
}